# -*- coding = utf-8 -*-
# @Time : 2023/9/12 9:36
# @Author : 龙王赘婿_彪少
# @File : PTC200.py
# @Software: PyCharm

# 功能：PTC200 保存encoder的值


from socket import *
from threading import Thread
import struct
import time
import binascii
from datetime import datetime, timedelta

class MYTCP:
    current_time = datetime.now()
    last_time = datetime.now()
    last_encoder = datetime.now()

    count = 0

    def __init__(self):
        # self.ip = "192.168.8.31"
        self.ip = "192.168.111.250"
        self.port = 9002
        self.client_socket = socket(AF_INET, SOCK_STREAM)
        self.client_socket.connect((self.ip, self.port))
        print("connected")
        print(datetime.now())
        t1 = Thread(target=self.recv_thread)
        t1.start()
        t1.join()
        self.client_socket.close()

    # 接收线程
    def recv_thread(self):
        self.f = open('../file/encoder.txt', 'w+')
        while True:
            recv_data = self.client_socket.recv(1024)  # 最大接收1024字节
            self.print_recv_data(recv_data)
            self.decode_encoder_data(recv_data)

    @staticmethod
    def print_recv_data(raw_data):
        print(datetime.now(), f"recv：len = {len(raw_data)} data =", end=" ")
        for i in range(len(raw_data)):
            print(f"{raw_data[i]:02x}", end=" ")
        print()

    # 解析编码器和速度报文
    def decode_encoder_data(self, raw_data: bytes):
        if raw_data[0] == 0xff and raw_data[1] == 0xfe and raw_data[2] == 0x03 and raw_data[3] == 0x00:
            s = struct.Struct("<2B2HI8f4i1H2B")
            up = s.unpack(raw_data[0:62])
            head = up[0]
            version = up[1]
            type = up[2]
            len = up[3]
            count = up[4]

            speed_lf = up[5]
            speed_rf = up[6]
            speed_lb = up[7]
            speed_rb = up[8]

            degrees_lf = up[9]
            degrees_rf = up[10]
            degrees_lb = up[11]
            degrees_rb = up[12]

            line_lf = up[13]
            line_rf = up[14]
            line_lb = up[15]
            line_rb = up[16]

            crc = up[17]
            end1 = up[18]
            end2 = up[19]

            self.f.write(f"{datetime.now()} speed: lf = \t{speed_lf:f}\t rf = \t{speed_rf:f}\t lb = \t{speed_lb:f}\t rb = \t{speed_rb:f}\t"
                         f" line: lf = \t{line_lf:d}\t rf = \t{line_rf:d}\t lb = \t{line_lb:d}\t rb = \t{line_rb:d}\t\n")

            # print(f"head = {head:02x} version = {version:02x} type = {type:04x}")
            # print(f"speed: lf = {speed_lf:f} rf = {speed_rf:f} lb = {speed_lb:f} rb = {speed_rb:f}")
            # print(f"degree: lf = {degrees_lf:f} rf = {degrees_rf:f} lb = {degrees_lb:f} rb = {degrees_rb:f}")
            # print(f"line: lf = {line_lf:d} rf = {line_rf:d} lb = {line_lb:d} rb = {line_rb:d}")

    @staticmethod
    def crc16_cal(datalist):
        # from "https://code84.com/461550.html"
        test_crc = 0xFFFF
        poly = 0xa001
        # poly=0x8005
        numl = len(datalist)
        for num in range(numl):
            data = datalist[num]
            test_crc = (data & 0xFF) ^ test_crc
            for bit in range(8):
                if (test_crc & 0x1) != 0:
                    test_crc >>= 1
                    test_crc ^= poly
                else:
                    test_crc >>= 1
        return test_crc


if __name__ == "__main__":
    obj = MYTCP()
